'''
Description: navigation
Version: V1.0
Author: zw_1520@163.com
Date: 2025-04-19 16:07:02
LastEditors: zw_1520@163.com
LastEditTime: 2025-05-05 11:16:42
All rights reserved.
'''
#!/usr/bin/env python3

import argparse
import os
import signal
import sys
import threading
import time

import aimrt_py
import agibot_controller.navigation_module
import json

global_aimrt_core = None
exit_event = threading.Event()
exit_event.set()


def signal_handler(sig, _):
    global global_aimrt_core, exit_event
    print(f"Received signal {sig}, shutting down...")
    exit_event.clear()
    agibot_controller.navigation_module.running.clear()

    if global_aimrt_core and (sig == signal.SIGINT or sig == signal.SIGTERM):
        global_aimrt_core.Shutdown()
        return
    sys.exit(0)


def main():
    # Parse arguments
    parser = argparse.ArgumentParser(description="Navigation module example.")
    parser.add_argument("--cfg_file_path", type=str,
                        default="", help="config file path")
    args = parser.parse_args()

    # Register signal handlers
    signal.signal(signal.SIGINT, signal_handler)
    signal.signal(signal.SIGTERM, signal_handler)

    print("AimRT start.")

    # Check config file
    if args.cfg_file_path and not os.path.exists(args.cfg_file_path):
        print(f"Warning: Config file '{args.cfg_file_path}' does not exist!")

    # Initialize core
    aimrt_core = aimrt_py.Core()
    global global_aimrt_core
    global_aimrt_core = aimrt_core

    # Create and register module
    module = agibot_controller.navigation_module.NavigationModule()
    aimrt_core.RegisterModule(module)

    # Initialize and start
    core_options = aimrt_py.CoreOptions()
    core_options.cfg_file_path = args.cfg_file_path
    aimrt_core.Initialize(core_options)

    thread = threading.Thread(target=aimrt_core.Start)
    thread.daemon = True
    thread.start()

    # Wait for exit
    try:
        while exit_event.is_set() and thread.is_alive():
            time.sleep(0.1)
    except KeyboardInterrupt:
        exit_event.clear()
        agibot_controller.navigation_module.running.clear()

    # Shutdown
    aimrt_core.Shutdown()
    thread.join(timeout=3.0)

    print("AimRT exit.")


def navi_to_target(config_file_path, target_id):
    # Parse arguments
    # parser = argparse.ArgumentParser(description="Navigation module example.")
    # parser.add_argument("--cfg_file_path", type=str, default="", help="config file path")
    # args = parser.parse_args()

    # Register signal handlers
    # signal.signal(signal.SIGINT, signal_handler)
    # signal.signal(signal.SIGTERM, signal_handler)

    print("AimRT start.")

    # Check config file
    if config_file_path and not os.path.exists(config_file_path):
        print(f"Warning: Config file '{config_file_path}' does not exist!")

    # Initialize core
    aimrt_core = aimrt_py.Core()
    global global_aimrt_core
    global_aimrt_core = aimrt_core

    # Create and register module
    module = agibot_controller.navigation_module.NavigationModule()
    module.set_target_id(target_id)
    aimrt_core.RegisterModule(module)

    # Initialize and start
    core_options = aimrt_py.CoreOptions()
    core_options.cfg_file_path = config_file_path
    aimrt_core.Initialize(core_options)

    thread = threading.Thread(target=aimrt_core.Start)
    thread.daemon = True
    thread.start()

    # Wait for exit
    # try:
    #     while thread.is_alive():
    #         # print("Wait exit event.")
    #         time.sleep(0.1)
    # except KeyboardInterrupt:
    #     exit_event.clear()
    #     agibot_controller.navigation_module.running.clear()

    # Shutdown
    aimrt_core.Shutdown()
    thread.join(timeout=3.0)

    print("AimRT exit.")


def navi_task_process(config_file_path, navi_param):
    print(config_file_path)
    print(navi_param)
    data = json.loads(navi_param)
    print(data["poi"]["id"])
    navi_to_target(config_file_path, data["poi"]["id"])


if __name__ == "__main__":
    main()
